#ifndef ALGORITHM_H
#define	ALGORITHM_H

#include <queue>
#include <cmath>
#include <stdint.h>
#include <iostream>
#include <set>
#include <algorithm>

#include "GameState.h"
#include "Common.h"

using namespace std;

class Algorithm
{
private:
    typedef struct _asCoord
    {
        Coord current;
        Coord prev;
        int32_t g;
        int32_t h;
        int32_t f;
        bool operator == (const _asCoord & crd) const 
        {
            return (current.row == crd.current.row) && (current.column == crd.current.column);
        }
    } ASCoord;
    class ASCoordSetCmp
    {
    public:
        bool operator ()(const ASCoord & c1, const ASCoord & c2) const
        {
            return (c1.current.row < c2.current.row) ||
                    ((c1.current.row == c2.current.row) && (c1.current.column < c2.current.column));
        }
    };
    
    class CoordSetCmp
    {
    public:
        bool operator ()(const Coord & c1, const Coord & c2) const
        {
            return (c1.row < c2.row) || ((c1.row==c2.row) && (c1.column < c2.column));
        }
    };
    
public:

    static int32_t getManhDistance(int32_t r1, int32_t c1, int32_t r2, int32_t c2)
    {
        return static_cast<int32_t> (abs(r1-r2) + abs(c1-c2));
    }
    
    static int32_t getManhDistance(const ACell * begin, const ACell * end)
    {
        return getManhDistance(begin->row(), begin->column(), end->row(), end->column());
    }
    static int32_t getManhDistance(const ACell & begin, const ACell & end)
    {
        return getManhDistance(begin.row(), begin.column(), end.row(), end.column());
    }
    // Находит ближйшего муравья к еде
    static bool findCellWithType(GameState & st, const int32_t & beginRow, const int32_t & beginColumn, 
                    int32_t & endRow, int32_t & endColumn, CellType toFind)
    {
        const char moves[] = {'w', 's', 'e', 'n'};
        set<Coord, CoordSetCmp> seenCell;
        Coord addCell;
        
        queue<Coord> bufferToSee;
        addCell = {beginRow, beginColumn};
        bufferToSee.push(addCell);
        
        //TODO возможно имеет смысл ограничить глубину рекурсии
        while (bufferToSee.size() != 0)
        { 
            Coord currentCell = bufferToSee.front();
            seenCell.insert(currentCell);
            bufferToSee.pop();

            // вывожу уже просмотренные клетки
            if (st.getXY(currentCell.row, currentCell.column)->getType() == toFind)
            {
                /*
                 * Если мы ищем муравья и это НЕ МОЙ муравей, то это не то что нам надо
                 */
                if (toFind == ANT)
                {
                    Ant * ant = reinterpret_cast<Ant*>(st.getXY(currentCell.row, currentCell.column));
                    if (!ant->isMyAnt())
                    {
                        continue;
                    }
                }
                
                endRow = currentCell.row;
                endColumn = currentCell.column;
                // мы нашли ближайшего муравья
                return true;
            }
            else
            {
                // заносим 4 ближайших соседей в очередь (кроме рассмотренных уже)
                
                // формируем массив из <= 4 СВОБОДНЫХ(!) соседей
                vector<Coord> neighbors;
                int32_t neighborRow, neighborColumn;
                
                for (char move : moves)
                {
                    st.getXYAfterMove(currentCell.row, currentCell.column, neighborRow, neighborColumn, move);
                    
                    if (st.getXY(neighborRow, neighborColumn)->getType() != WATER)
                    {
                        Coord toAdd = {neighborRow, neighborColumn};
                        neighbors.push_back(toAdd);
                    }
                }
                
                for (auto n : neighbors)
                {
                    if (seenCell.count(n) == 0)
                    {
                        /*
                         * Доп. проверка. Если:
                         * 1) мы ищем муравья
                         * 2) это мой муравей, который идёт за едой,
                         *      то он не проходим
                         */
                        if (toFind == ANT)
                        {
                            ACell * newCell = st.getXY(n.row, n.column);
                            if (newCell->getType() == ANT)
                            {
                                Ant * ant = reinterpret_cast<Ant*>(newCell);
                                if (ant->isMyAnt())
                                {
                                    if (ant->isBusy())
                                    {
                                        // ну вот такого муравья мы не считаем за человека. А остальные - те кто нам нужны
                                        continue;
                                    }
                                }
                            }
                        }
                        /*
                         * Вторая проверка. Если:
                         * 1) мы ищем не муравья, то муравьи не проходимы и мы рассматриваем другого соcеда
                         */
                        else
                        {
                            ACell * newCell = st.getXY(n.row, n.column);
                            if (newCell->getType() == ANT)
                            {
                                continue;
                            }
                        }
                        seenCell.insert(n);
                        bufferToSee.push(n);
                    }
                }
                
            }
        } //big while
        
        // если мы сюда дошли, значит всё плохо
        endColumn = -1;
        endRow = -1;
        return false;
    }

    
    static bool findASPath(GameState & st, const Ant & begin, int32_t endRow, int32_t endColumn, char & antMove)
    {
        // TODO тут вообще добавить проверку на проходимость финальной клетки
        const char moves[] = {'n', 'w', 's', 'e'};
        // уже рассмотренные вершины
        set<ASCoord, ASCoordSetCmp> closed;    
        vector<ASCoord> opened;
        //sort(opened.begin(), opened.end(), [](const ASCoord & c1, const ASCoord & c2){return c1.f < c2.f;});
        ASCoord start = {{begin.row(), begin.column()},{-1, -1},0,0,0};
        start.g = 0;
        start.h = getManhDistance(begin.row(), begin.column(), endRow, endColumn);
        start.f = start.g + start.h;
        
        opened.push_back(start);
        
        while (opened.size() != 0)
        {
            /*
             * Внимание! Элементы с минимальным f хранятся в конце (чтобы легче доставать из vector было!!!
             */
            sort(opened.begin(), opened.end(), [](const ASCoord & c1, const ASCoord & c2){return c1.f > c2.f;});
            
            ASCoord x = opened.back();
            
            if ((x.current.row == endRow) && (x.current.column == endColumn))
            {
                // путь найден
                //cerr << "Path is found!" << endl;
                // реконструирем путь
                vector<ASCoord> path;
                path.push_back(x);
                
                ASCoord * currentNode = &path.back();
                
                while((currentNode->prev.row != -1) && (currentNode->prev.column != -1))
                {
                    ASCoord tmp = {{currentNode->prev.row, currentNode->prev.column}, {0,0},0,0,0};
                    auto it = find(closed.begin(), closed.end(), tmp);
                    const ASCoord & lCoord = *it;
                    path.push_back(lCoord);
                    currentNode = &path.back();
                }
                
                /*
                 * В массиве path у нас хранится путь с конца в начало
                 * В начале пути у нас муравей - нам надо сказать муравью, какой ему делать шаг
                 */
                auto lastStep = path.at(path.size() - 1);
                ACell * c1 = st.getXY(lastStep.current.row, lastStep.current.column);
                auto nextStep = path.at(path.size() - 2);
                ACell * c2 = st.getXY(nextStep.current.row, nextStep.current.column);
                
                antMove = st.getMove(c1, c2);
                
                
                /*cerr << "Path: ";
                for (auto step : path)
                {
                    cerr << '{' << step.current.row << ',' << step.current.column <<'}';
                }
                cerr << endl;*/
                return true;
            }
            
            opened.pop_back();
            
            closed.insert(x);
            
            // формируем массив из <= 4 СВОБОДНЫХ(!) соседей
            vector<Coord> neighbors;
            int32_t neighborRow, neighborColumn;
                
            for (char move : moves)
            {
                st.getXYAfterMove(x.current.row, x.current.column, neighborRow, neighborColumn, move);
                if (st.getXY(neighborRow, neighborColumn)->isEmpty())
                {
                    // моих муравьёв тоже рассматриваем не проходимыми
                    
                    Coord toAdd = {neighborRow, neighborColumn};
                    neighbors.push_back(toAdd);
                }
            }
            
            for (auto n : neighbors)
            {
                ASCoord y = {{n.row, n.column},{x.current.row,x.current.column},0,0,0};
                if (closed.count(y))
                {
                    // соседей из закрытого списка пропускаем
                    continue;
                }
                
                int32_t tenativeGScore = x.g + 1;
                
                auto yInOpened = find(opened.begin(), opened.end(), y);

                if (yInOpened == opened.end())
                {
                    // y нет в opened , Добавляем
                    y.prev.row = x.current.row;
                    y.prev.column = x.current.column;
                    y.g = tenativeGScore;
                    y.h = getManhDistance(y.current.row, y.current.column, endRow, endColumn);
                    y.f = y.h + y.g;
                    opened.push_back(y);
                }
                else
                {
                    // y есть в opened. НО!
                    if (tenativeGScore < yInOpened->g)
                    {
                        // вычисленная G оказалась лучше, т.е. пудем обновлять путь
                        yInOpened->prev.row = x.current.row;
                        yInOpened->prev.column = x.current.column;
                        yInOpened->g = tenativeGScore;
                        yInOpened->h = getManhDistance(yInOpened->current.row, yInOpened->current.column,
                                                        endRow, endColumn);
                        yInOpened->f = y.h + y.g;
                    }
                    else
                    {
                        // вычисленная G хуже, т.е. существует уже лучший путь
                    }
                }
                
            }

            
        }
        
        
        return false;
    }
};

    

#endif	/* ALGORITHM_H */

